<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
        "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" xml:lang="en" lang="en">
	<head>
	        <!-- charset must remain utf-8 to be handled properly by Processing -->
		<meta http-equiv="content-type" content="text/html; charset=utf-8" />
		
		<title>PROCESSING_FUNCTIONS : Built with Processing</title>
		
		<style type="text/css">
		/* <![CDATA[ */
	
		body {
  		  margin: 60px 0px 0px 55px;
		  font-family: verdana, geneva, arial, helvetica, sans-serif; 
		  font-size: 11px; 
		  background-color: #ddddcc; 
		  text-decoration: none; 
		  font-weight: normal; 
		  line-height: normal; 
		}
		 
		a          { color: #3399cc; }
		a:link     { color: #3399cc; text-decoration: underline; }
		a:visited  { color: #3399cc; text-decoration: underline; }
		a:active   { color: #3399cc; text-decoration: underline; }
		a:hover    { color: #3399cc; text-decoration: underline; }
	
		/* ]]> */
		</style>
	 
	</head>
	<body>
		<div id="content">
			<div id="PROCESSING_FUNCTIONS_container">
			
			<!--[if !IE]> -->
				<object classid="java:PROCESSING_FUNCTIONS.class" 
            			type="application/x-java-applet"
            			archive="PROCESSING_FUNCTIONS.jar"
            			width="100" height="100"
            			standby="Loading Processing software..." >
            			
					<param name="archive" value="PROCESSING_FUNCTIONS.jar" />
				
					<param name="mayscript" value="true" />
					<param name="scriptable" value="true" />
				
					<param name="image" value="loading.gif" />
					<param name="boxmessage" value="Loading Processing software..." />
					<param name="boxbgcolor" value="#FFFFFF" />
				
					<param name="test_string" value="outer" />
			<!--<![endif]-->
				
				<object classid="clsid:8AD9C840-044E-11D1-B3E9-00805F499D93" 
						codebase="http://java.sun.com/update/1.5.0/jinstall-1_5_0_15-windows-i586.cab"
						width="100" height="100"
						standby="Loading Processing software..."  >
						
					<param name="code" value="PROCESSING_FUNCTIONS" />
					<param name="archive" value="PROCESSING_FUNCTIONS.jar" />
					
					<param name="mayscript" value="true" />
					<param name="scriptable" value="true" />
					
					<param name="image" value="loading.gif" />
					<param name="boxmessage" value="Loading Processing software..." />
					<param name="boxbgcolor" value="#FFFFFF" />
					
					<param name="test_string" value="inner" />
					
					<p>
						<strong>
							This browser does not have a Java Plug-in.
							<br />
							<a href="http://www.java.com/getjava" title="Download Java Plug-in">
								Get the latest Java Plug-in here.
							</a>
						</strong>
					</p>
				
				</object>
				
			<!--[if !IE]> -->
				</object>
			<!--<![endif]-->
			
			</div>
			
			<p>
			void DogaPrintMatrixCol(String matrixName,float[][] Matrix,int row,int col/*PRINTS IN COL ORDER*/) {
println("Printing the matrix = "+matrixName+" in coloumn based order");
for (int i=0;i<row;i++) {
	for (int j=0;j<col;j++) {
		print("["+Matrix[i+j*row]+"],");
	}
	println();
}
println();
return;
}
void DogaPrintMatrixRow(String matrixName,float[][] Matrix,int row,int col/*PRINTS IN COL ORDER*/) {
println("Printing the matrix = "+matrixName+" in row based order");
for (int i=0;i<row;i++) {
	for (int j=0;j<col;j++) {
		print("["+Matrix[j+i*col]+"],");
	}
	println();
}
println();
return;
}
void DogaCopyMatrix(float[] from,float[] to,int n){
int i=0;
for (i=0;i<n;i++) to[i]=from[i];
return;
}
void DogaCopyMatrix(float[][] from,float[][] to,int row,int col){
int i=0,j=0;
for (i=0;i<row;i++)
for(j=0;j<col;j++)
to[i][j]=from[i][j];
return;
}
float DogaDotProduct(float V1[],float V2[]) {return V1[0]*V2[0]+V1[1]*V2[1]+V1[2]*V2[2];}
float[] DogaCrossProduct(float V1[],float V2[]) {
float m;
float[] Vn=new float[4];
Vn[0] = V1[1]*V2[2] - V2[1]*V1[2];
Vn[1] = V2[0]*V1[2] - V1[0]*V2[2];
Vn[2] = V1[0]*V2[1] - V2[0]*V1[1];
Vn[3] = 1;
m = sqrt(DogaDotProduct(Vn,Vn));
if (m==0) {
println("**BE CAREFUL THE LINES ARE PARALLEL TO EACH OTHER***");
for (int i=0;i<3;i++) Vn[i] = 0;
}
else for (int i=0;i<3;i++) Vn[i] = Vn[i]/m;
return Vn;
}
float DogaCrossProduct(float V1[],float V2[],float ApplySolution[]) {
float m;
float[] Vn=new float[4];
Vn[0] = V1[1]*V2[2] - V2[1]*V1[2];
Vn[1] = V2[0]*V1[2] - V1[0]*V2[2];
Vn[2] = V1[0]*V2[1] - V2[0]*V1[1];
Vn[3] = 1;
m = sqrt(DogaDotProduct(Vn,Vn));
if (m==0) {
println("**BE CAREFUL THE LINES ARE PARALLEL TO EACH OTHER***");
for (int i=0;i<3;i++) ApplySolution[i] = 0;
}
else for (int i=0;i<3;i++) ApplySolution[i] = Vn[i]/m;
ApplySolution[3]=1;
return m;
}
float DogaMakeUnitVector(float V[]) {
float w=0;
w = sqrt(DogaDotProduct(V,V));
if (w!=0) {
V[0] = V[0]/w;
V[1] = V[1]/w;
V[2] = V[2]/w;
}
else {
V[0] = 0;
V[1] = 0;
V[2] = 0;
}
return w;
}
float[] DogaReturnUnitVector(float V[]) {
float w=0,retV[]=new float[4];
w = sqrt(DogaDotProduct(V,V));
if (w!=0) {
retV[0] = V[0]/w;
retV[1] = V[1]/w;
retV[2] = V[2]/w;
}
else {
retV[0] = 0;
retV[1] = 0;
retV[2] = 0;
}
retV[3] = 1;
return retV;
}
void DogaLoadIdentity(float[][] Matrix,int row,int col) {
int i=0,j=0;
for (i=0;i<row;i++)
for (j=0;j<col;j++)
if (i==j) Matrix[i][j]=1; else Matrix [i][j]=0;
}

float[][] DogaMatrixMultCol(float[][] matA,float[][] matB,int A,int B,int C) {
float sol[][]=new float[A][C];
int i,j,k;
for (i=0;i<A;i++) {
for (j=0;j<C;j++) {
sol[i][j] = 0;
for (k=0;k<B;k++)
	sol[i][j] += matA[i][k]*matB[k][j];
}
}
return sol;
}

void DogaMatrixMultCol(float[][] matA,float[][] matB,int A,int B,int C,float[][] ApplySolution) {
float sol[][]=new float[A][C];
int i,j,k;
for (i=0;i<A;i++) {
for (j=0;j<C;j++) {
sol[i][j] = 0;
for (k=0;k<B;k++)
	sol[i][j] += matA[i][k]*matB[k][j];
}
}
for (i=0;i<A;i++)
for (j=0;j<C;j++)
ApplySolution[i][j]=sol[i][j]; 
return;
}
float[][] DogaMatrixMultRow(float[][] matA,float matB[][],int A,int B,int C) {
float sol[][]=new float[A][C];
int i,j,k;
// matA will be AxB matrix ; matB will be BxC matrix ; solution will be AxC matrix
for (i=0;i<A;i++)
for (j=0;j<C;j++)
sol[i][j] = 0;
for (i=0;i<A;i++) {//A rows(points) of matA
for (j=0;j<B;j++) {//B coloumns of matA and rows of matB
for (k=0;k<C;k++) //C cols of matB
	sol[i][j] += matA[i][k]*matB[j][k];
}
}
return sol;
}
void DogaMatrixMultRow(float[][] matA,float matB[][],int A,int B,int C,float[][] ApplySolution) {
float sol[][]=new float[A][C];
int i,j,k;
// matA will be AxB matrix ; matB will be BxC matrix ; solution will be AxC matrix
for (i=0;i<A;i++)
for (j=0;j<C;j++)
sol[i][j] = 0;
for (i=0;i<A;i++) {//A rows(points) of matA
for (j=0;j<B;j++) {//B coloumns of matA and rows of matB
for (k=0;k<C;k++) //C cols of matB
	sol[i][j] += matA[i][k]*matB[j][k];
}
}
for (i=0;i<A;i++)
for (j=0;j<C;j++)
ApplySolution[i][j] = sol[i][j];
return;
}
int maxNumOfRootChains=8,maxNumOfChains=10;
int funcCCD=0,funcCIR=1;
int TRACKER=0,CLICKER=1,ARBITRARY=2;
int CIRCLEmove=0,SQUAREmove=1,RANDOMmove=2;

int mode=TRACKER,usedFunction=funcCCD;
int moveMode=CIRCLEmove,moveCnt=0;

int screenW=800,screenH=800;
float mouseSensitivity=0,minSensitivity=0,maxSensitivity=1500;
int myMouseX=0,myMouseY=0;

float P[][][] = new float[maxNumOfRootChains][maxNumOfChains][4];
float L[][] = new float[maxNumOfRootChains][maxNumOfChains];
float A[][] = new float[maxNumOfRootChains][maxNumOfChains];
float D1[]= new float[]{screenW/2,screenH/3,-10,1};

int numOfRootChains=1;
float rootsDif=20;
float totalLength[]=new float[maxNumOfRootChains],maxChainLength,minChainLength,chainLengthDif;
int nChain[]=new int[maxNumOfRootChains],minChain=2;
int cnt[]=new int[maxNumOfRootChains],maxCnt=10;

boolean rootSelect=false,chainSelect=false,amountSelect=false;
int selectedRoot=0,selectedChain=1,amount=0;

void randomMoveFunc() {
float degree=(mouseSensitivity/300),X,Y;
moveCnt=(moveCnt+1)%360;
if (moveMode==CIRCLEmove) {
if (moveCnt%50!=0) return;
X = D1[0]-screenW/2;
Y = D1[1]-screenH/2;
if (degree==0) degree=1;
D1[0] = X*cosinus(degree) - Y*sinus(degree) + screenW/2;
D1[1] = X*sinus(degree) + Y*cosinus(degree) + screenH/2;
for (int i=0;i<numOfRootChains;i++) cnt[i]=0;
}
if (moveMode==SQUAREmove) return;
if (moveMode==RANDOMmove) return;
}

void cirIterate(int Pindex,float[] lineLength) {
int i=0;
float V1[]=new float[]{-P[Pindex][0][0]+D1[0],-P[Pindex][0][1]+D1[1],0};
float midPoint[]=new float[]{(P[Pindex][0][0]+D1[0])/2,(P[Pindex][0][1]+D1[1])/2,0};
float V2[]=new float[]{1,0,0};
float V3[],V4[]=new float[4],Length,degree,Vk[]=new float[3],len,xLen;
Length = sqrt(DogaDotProduct(V1,V1));
lineLength[0]=Length;
V3 = DogaReturnUnitVector(V1);
DogaCrossProduct(V2,V3,V4);
degree = rad2deg(acos(DogaDotProduct(V3,V2)));
if (V4[2]<0) degree=360-degree;
for (i=0;i<=nChain[Pindex];i++) A[Pindex][i]=degree;
V4[0]=0; V4[1]=0; V4[2]=0; V4[3]=0;
if (Length<=totalLength[Pindex]) {
xLen = Length/nChain[Pindex];
DogaCopyMatrix(P[Pindex][0],Vk,3);
for (i=1;i<=nChain[Pindex];i++) {
Vk[0] += xLen*V3[0];
Vk[1] += xLen*V3[1];
V4[0] = Vk[0] - midPoint[0];
V4[1] = Vk[1] - midPoint[1];
len = sqrt(DogaDotProduct(V4,V4));
P[Pindex][i][1] = Vk[1];
P[Pindex][i][0] = Vk[0]-sqrt(Length*Length*0.25-len*len)*((Length-totalLength[Pindex])/totalLength[Pindex]);
P[Pindex][i][2] = -10;
}
}
}
float ccdIterate(float[] center,float[] end) {
float V1[]=new float[]{-center[0]+D1[0],-center[1]+D1[1],0,1};
float V2[]=new float[]{-center[0]+end[0],-center[1]+end[1],0,1};
float V3[],V4[],V5[]=new float[4],degree;
V3 = DogaReturnUnitVector(V1);
V4 = DogaReturnUnitVector(V2);
DogaCrossProduct(V1,V2,V5);
degree = rad2deg(acos(DogaDotProduct(V3,V4)));
degree = int(degree)%360;
if (V5[2]>0) degree*=-1;
return degree;
}
boolean done(float P1,float P2,float dif) {
if (absD(P1-P2)<dif) return true;
return false;
}
void drawMyIK() {
int i=0,j=0;
for (i=0;i<numOfRootChains;i++) {
stroke(1,1,1);
noFill();
ellipseMode(CENTER);
ellipse(P[i][0][0],height-P[i][0][1],totalLength[i]*2,totalLength[i]*2);
stroke(255,255,255);
fill(255,255,0);
for (j=1;j<=nChain[i];j++) {
line(P[i][j-1][0],height-P[i][j-1][1],P[i][j][0],height-P[i][j][1]);
ellipse(P[i][j-1][0],height-P[i][j-1][1],10,10);
}
ellipse(P[i][j-1][0],height-P[i][j-1][1],10,10);
}
}
void myIK() {
int i=0,j=0,k=0;
float lineLength[]=new float[1];
for (i=0;i<numOfRootChains;i++) {
if ((cnt[i]<=maxCnt)&&(usedFunction==funcCCD))
for (j=nChain[i];j>0;j--) {
//FIND THE ALFA VALUES FOR EACH ONE AT FIRST
if (done(D1[0],P[i][nChain[i]][0],0.4)&&done(D1[1],P[i][nChain[i]][1],0.4)) cnt[i]=maxCnt+1;
if (cnt[i]<=maxCnt) {
A[i][j] += ccdIterate(P[i][j-1],P[i][nChain[i]]);
//ITERATIVELY FIND ALL THE UPDATED POINTS
for(k=1;k<=nChain[i];k++){
P[i][k][0] = P[i][k-1][0] + L[i][k]*cosinus(A[i][k]);
P[i][k][1] = P[i][k-1][1] + L[i][k]*sinus(A[i][k]);
P[i][k][2] = -10;
}
}
}
else if ((cnt[i]<=maxCnt)&&(usedFunction==funcCIR)){
cirIterate(i,lineLength);
//ITERATIVELY FIND ALL THE UPDATED POINTS
if (lineLength[0]>totalLength[i]) 
for(k=1;k<=nChain[i];k++){
P[i][k][0] = P[i][k-1][0] + L[i][k]*cosinus(A[i][k]);
P[i][k][1] = P[i][k-1][1] + L[i][k]*sinus(A[i][k]);
P[i][k][2] = -10;
}
}
cnt[i]++;
}
}

void setup() {
int i=0,j=0;
size(screenW,screenH);
resetValues(true);
}
void draw() {
int i=0;
float dif=0;
background(0,50,100);
myMouseX=mouseX;
myMouseY=height-mouseY;
if (mode==TRACKER) {
dif = absD((D1[0]-myMouseX)*(D1[0]-myMouseX)+(D1[1]-myMouseY)*(D1[1]-myMouseY));
if (dif>mouseSensitivity) {
D1[0] = myMouseX;
D1[1] = myMouseY;
for (i=0;i<numOfRootChains;i++)	
cnt[i]=0;
}
}
else if (mode==ARBITRARY) randomMoveFunc();
myIK();
drawMyIK();
stroke(255,255,255);
fill(255,0,0);
ellipseMode(CENTER);
ellipse(D1[0],height-D1[1],10,10);
}

void mousePressed(){
float dif=0;
int i=0;
myMouseX=mouseX;
myMouseY=height-mouseY;
dif = absD((D1[0]-myMouseX)*(D1[0]-myMouseX)+(D1[1]-myMouseY)*(D1[1]-myMouseY));
if ((dif>mouseSensitivity)&&(mode==CLICKER)) {
D1[0] = myMouseX;
D1[1] = myMouseY;
for (i=0;i<numOfRootChains;i++)	
cnt[i]=0;
}
}

void keyPressed(){
int num=0;
if ((key>='0')&&(key<='9')) {
num=key-'0';
if (rootSelect) {
selectedRoot=num;
println("SelectedRoot= "+selectedRoot);
}
if (chainSelect) {
selectedChain=num;
println("SelectedChain="+selectedChain);
}
if (amountSelect) {
amount=amount*10+num;
println("Amount="+amount);
}
}
else amount=0;
if ((key=='q')||(key=='Q')) {
rootSelect=true;
chainSelect=false;
amountSelect=false;
}
else if ((key=='w')||(key=='W')) {
rootSelect=false;
chainSelect=true;
amountSelect=false;
}
else if ((key=='e')||(key=='E')) {
rootSelect=false;
chainSelect=false;
amountSelect=true;
}
else if (key=='R') {//add one root
if (numOfRootChains<maxNumOfRootChains) {
numOfRootChains++;
resetValues(true);
}
}
else if (key=='r') {//delete one root
if (numOfRootChains>1) {
numOfRootChains--;
resetValues(true);
}
}
else if (key=='J') {//add one joint
if (nChain[0]<maxNumOfChains-1) {
for (int i=0;i<numOfRootChains;i++)
	nChain[i]++;
resetValues(false);
}
}
else if (key=='j') {//delete one root
if (nChain[0]>minChain) {
for (int i=0;i<numOfRootChains;i++)
	nChain[i]--;
resetValues(false);
}
}
}

void resetValues(boolean rootChainReset) {
int i=0,j=0;
float deg;
if (rootChainReset) {
if (numOfRootChains<=4) {
maxChainLength = screenW/2;
minChainLength = screenW/4;
}
else {
maxChainLength = screenW/4;
minChainLength = screenW/8;
}
chainLengthDif = screenW/8;
for (i=0;i<numOfRootChains;i++) {
if (rootChainReset) totalLength[i] = minChainLength;
if (rootChainReset) nChain[i] = (maxNumOfChains-numOfRootChains)/2+1;
}
}
if (numOfRootChains==1) {
P[0][0][0] = screenW/2;
P[0][0][1] = screenH/2;
P[0][0][2] = -10;
P[0][0][3] = 1;
for (j=0;j<=nChain[0];j++) A[0][j]=270;
}
else {
deg = 360/numOfRootChains;
for (i=0;i<numOfRootChains;i++) {
P[i][0][0] = screenW/2+(cosinus(i*deg))*(0.5*screenW-(numOfRootChains-2)*rootsDif);
P[i][0][1] = screenH/2+(sinus(i*deg))*(0.5*screenH-(numOfRootChains-2)*rootsDif);
P[i][0][2] = -10;
P[i][0][3] = 1;
for (j=0;j<=nChain[i];j++) A[i][j]=0;
}
}
for (i=0;i<numOfRootChains;i++) {
L[i][0] = totalLength[i]/nChain[i];
for (j=1;j<=nChain[i];j++) {
L[i][j] = totalLength[i]/nChain[i];
P[i][j][0] = P[i][j-1][0] + L[i][j]*cosinus(A[i][j]);
P[i][j][1] = P[i][j-1][1] + L[i][j]*sinus(A[i][j]);
P[i][j][2] = -10;
P[i][j][3] = 1;
}
cnt[i]=0;
}
}

			</p>
			
			<p>
			Source code: <a href="PROCESSING_FUNCTIONS.pde">PROCESSING_FUNCTIONS</a> 
			</p>
			
			<p>
			Built with <a href="http://processing.org" title="Processing.org">Processing</a>
			</p>
		</div>
	</body>
</html>
